#include "link.h"
#include <vector>
#include <Eigen/Core>

using namespace Eigen;
using namespace std;

#ifndef LINKMANAGER_H
#define LINKMANAGER_H
class LinkManager{
	public:
		
		LinkManager();
		void initLink(Vector3f origin, float t, float p,  float l, float glb_t, float glb_p);
		void addLink(float t, float p,  float l, float glb_t, float glb_p);
		void calculate(Vector3f end);
		void display();
		Vector3f getEndPos();
		vector<Link*> links;
		Matrix<float, 2, 3> get2D3LJacobian(float theta1, float theta2, float theta3);
		Matrix<float, 3, 6> get3D3LJacobian(float theta1, float theta2, float theta3, float phi1, float phi2, float phi3);
		void draw(Vector3f pos, float theta, float phi, float length);
		Vector3f p(float theta1, float theta2, float theta3, float phi1, float phi2, float phi3);
		Vector3f pi(Matrix<float, 6, 1> v, Vector3f end);		
	    void drawCube(float x , float y , float z);
		
};
#endif
